#ifndef NTPG_H_
#define NTPG_H_

#include <cmath>

typedef unsigned int uint;
typedef double ftype;

class ntpg
{
public:
	ntpg(uint NoImages, uint NoObjectPoints, uint NoCameras=1);
	virtual ~ntpg();

	// diplay functions
	void showMap();
	void showQuats(bool state = false);
	void showV(bool state = false);
	void showR(bool state = false);
	void showT(bool state = false);
	void showU(bool state = true);

	// After variables have initially been set, use these variables to change the values
	void setrx(uint i, ftype value) { _p[irx(i)] = value; }  // i=1.._N
	void setry(uint i, ftype value) { _p[iry(i)] = value; }  // i=1.._N
	void setrz(uint i, ftype value) { _p[irz(i)] = value; }  // i=1.._N

	void settx(uint i, ftype value) { _p[itx(i)] = value; }  // i=1.._M
	void setty(uint i, ftype value) { _p[ity(i)] = value; }  // i=1.._M
	void settz(uint i, ftype value) { _p[itz(i)] = value; }  // i=1.._M

	void setqw(uint i, ftype value) { _p[iqw(i)] = value; }  // i=1.._M
	void setqx(uint i, ftype value) { _p[iqx(i)] = value; }  // i=1.._M
	void setqy(uint i, ftype value) { _p[iqy(i)] = value; }  // i=1.._M
	void setqz(uint i, ftype value) { _p[iqz(i)] = value; }  // i=1.._M

	void setvx(uint i, ftype value) { _p[ivx(i)] = value; }  // i=1.._C
	void setvy(uint i, ftype value) { _p[ivy(i)] = value; }  // i=1.._C
	void setvz(uint i, ftype value) { _p[ivz(i)] = value; }  // i=1.._C

	void setux(uint i1, uint i2, ftype value) { _p[iux(i1,i2)] = value*xscale; }	// i1=1.._M, i2=1.._N
	void setuy(uint i1, uint i2, ftype value) { _p[iuy(i1,i2)] = value*yscale; }	// i1=1.._M, i2=1.._N

	void setr(uint i, ftype v1, ftype v2, ftype v3);  // i=1.._N
	void sett(uint i, ftype v1, ftype v2, ftype v3);  // i=1.._M
	void setq(uint i, ftype v0, ftype v1, ftype v2, ftype v3);  // i=1.._M
	void setv(uint i, ftype v1, ftype v2, ftype v3);  // i=1.._C
	void setu(uint i1, uint i2, ftype v1, ftype v2);  // i1=1.._M, i2=1.._N

	// state = 0 ==> unknown
	// state = 1 ==> known
	// These functions should be used to initially set the variables
	void setrx(uint i, ftype value, bool state);  // i=1.._N
	void setry(uint i, ftype value, bool state);  // i=1.._N
	void setrz(uint i, ftype value, bool state);  // i=1.._N

	void settx(uint i, ftype value, bool state);  // i=1.._M
	void setty(uint i, ftype value, bool state);  // i=1.._M
	void settz(uint i, ftype value, bool state);  // i=1.._M

	void setqw(uint i, ftype value, bool state);  // i=1.._M
	void setqx(uint i, ftype value, bool state);  // i=1.._M
	void setqy(uint i, ftype value, bool state);  // i=1.._M
	void setqz(uint i, ftype value, bool state);  // i=1.._M

	void setvx(uint i, ftype value, bool state);  // i=1.._C
	void setvy(uint i, ftype value, bool state);  // i=1.._C
	void setvz(uint i, ftype value, bool state);  // i=1.._C

	void setux(uint i1, uint i2, ftype value, bool state);	// i1=1.._M, i2=1.._N
	void setuy(uint i1, uint i2, ftype value, bool state);	// i1=1.._M, i2=1.._N

	void setk(uint i, uint value) { _k[i] = value; }	// i=1.._M and _k[i]=1.._C

	void setr(uint i, ftype v1, ftype v2, ftype v3, bool state);  // i=1.._N
	void sett(uint i, ftype v1, ftype v2, ftype v3, bool state);  // i=1.._M
	void setq(uint i, ftype v0, ftype v1, ftype v2, ftype v3, bool state);  // i=1.._M
	void setv(uint i, ftype v1, ftype v2, ftype v3, bool state);  // i=1.._C
	void setu(uint i1, uint i2, ftype v1, ftype v2, bool state);  // i1=1.._M, i2=1.._N

	void setfocalplaneresolution(ftype xResolution, ftype yResolution);

	void getRotMatrix(uint m, ftype *p, ftype& m11, ftype& m12, ftype& m13, ftype& m21, ftype& m22, ftype& m23, ftype& m31, ftype& m32, ftype& m33);
	void getRotMatrix(uint m, ftype& m11, ftype& m12, ftype& m13, ftype& m21, ftype& m22, ftype& m23, ftype& m31, ftype& m32, ftype& m33) { getRotMatrix(m,_p,m11,m12,m13,m21,m22,m23,m31,m32,m33); }
	void getQuat(uint m, ftype *p, ftype& q0, ftype& q1, ftype& q2, ftype& q3);
	void getQuat(uint m, ftype& q0, ftype& q1, ftype& q2, ftype& q3) { getQuat(m, _p, q0, q1, q2, q3); }
	void normQuat(uint m, ftype *p, ftype scale=1.0);
	void normQuat(uint m, ftype scale=1.0) { normQuat(m,_p,scale); }

	inline ftype& ux(uint m, uint n, ftype* p) { return p[iux(m,n)]; }
	inline ftype& uy(uint m, uint n, ftype* p) { return p[iuy(m,n)]; }

	inline ftype& rx(uint n, ftype* p) { return p[irx(n)]; }
	inline ftype& ry(uint n, ftype* p) { return p[iry(n)]; }
	inline ftype& rz(uint n, ftype* p) { return p[irz(n)]; }

	inline ftype& tx(uint m, ftype* p) { return p[itx(m)]; }
	inline ftype& ty(uint m, ftype* p) { return p[ity(m)]; }
	inline ftype& tz(uint m, ftype* p) { return p[itz(m)]; }

	inline ftype& qw(uint m, ftype* p) { return p[iqw(m)]; }
	inline ftype& qx(uint m, ftype* p) { return p[iqx(m)]; }
	inline ftype& qy(uint m, ftype* p) { return p[iqy(m)]; }
	inline ftype& qz(uint m, ftype* p) { return p[iqz(m)]; }

	inline ftype& vx(uint m, ftype* p) { return p[ivx(_k[m])]; }
	inline ftype& vy(uint m, ftype* p) { return p[ivy(_k[m])]; }
	inline ftype& vz(uint m, ftype* p) { return p[ivz(_k[m])]; }

	inline ftype& ux(uint m, uint n) { return ux(m,n,_p); }
	inline ftype& uy(uint m, uint n) { return uy(m,n,_p); }

	inline ftype& rx(uint n) { return rx(n,_p); }
	inline ftype& ry(uint n) { return ry(n,_p); }
	inline ftype& rz(uint n) { return rz(n,_p); }

	inline ftype& tx(uint m) { return tx(m,_p); }
	inline ftype& ty(uint m) { return ty(m,_p); }
	inline ftype& tz(uint m) { return tz(m,_p); }

	inline ftype& qw(uint m) { return qw(m,_p); }
	inline ftype& qx(uint m) { return qx(m,_p); }
	inline ftype& qy(uint m) { return qy(m,_p); }
	inline ftype& qz(uint m) { return qz(m,_p); }

	inline ftype& vx(uint m) { return vx(m,_p); }
	inline ftype& vy(uint m) { return vy(m,_p); }
	inline ftype& vz(uint m) { return vz(m,_p); }

	inline uint& irx(uint n) { return _map[n]; }
	inline uint& iry(uint n) { return _map[n+nry]; }
	inline uint& irz(uint n) { return _map[n+nrz]; }

	inline uint& itx(uint m) { return _map[m+ntx]; }
	inline uint& ity(uint m) { return _map[m+nty]; }
	inline uint& itz(uint m) { return _map[m+ntz]; }

	inline uint& iqw(uint m) { return _map[m+nqw]; }
	inline uint& iqx(uint m) { return _map[m+nqx]; }
	inline uint& iqy(uint m) { return _map[m+nqy]; }
	inline uint& iqz(uint m) { return _map[m+nqz]; }

	inline uint& ivx(uint m) { return _map[_k[m]+nvx]; }
	inline uint& ivy(uint m) { return _map[_k[m]+nvy]; }
	inline uint& ivz(uint m) { return _map[_k[m]+nvz]; }

	inline uint& iux(uint m, uint n) { return _map[(m-1)*_N+n+nux]; }
	inline uint& iuy(uint m, uint n) { return _map[(m-1)*_N+n+nuy]; }

	uint getT() { return _T; }
	uint getM() { return _M; }
	uint getN() { return _N; }
	uint getC() { return _C; }
	uint getU() { return _U; }
	ftype* getp() { return _p; }
	ftype getXfocalplaneresolution() { return 1.0/xscale; }
	ftype getYfocalplaneresolution() { return 1.0/yscale; }
	void globalImagePoint(uint m, ftype ux, ftype uy, ftype& x, ftype& y, ftype &z);
	void globalImagePixel(uint m, uint ux, uint uy, ftype& x, ftype& y, ftype &z);

	// the followng funtions are helper functions to orient the image m
	void orientImage(uint m, ftype wx, ftype wy, ftype wz, ftype angle_rot, bool state);
	void orientImage(uint m, ftype theta, ftype phi, ftype angle_rot, bool state);

	ftype func(ftype* p);
	ftype dfunc(ftype p[], ftype df[]);
	ftype Hessian(ftype p[], ftype** h);
	void solve(ftype ftol);

	void linmin(ftype *x, ftype smin, ftype smax, uint N);

protected:
	uint _M;		// Number of images
	uint _N;		// Number of object points
	uint _C;		// Number of cameras
	uint _U;		// Number of unknown variables
	uint _T;		// Total number of variables

	// define integer offsets
	uint nry, nrz, ntx, nty, ntz, nqw, nqx, nqy, nqz, nvx, nvy, nvz, nux, nuy;
	uint ik,iu;	// indicies to help map memory

	ftype *_p;	// _p[1.._U,_U+1.._T] linear array of all variables
	uint *_map;	// _map[1.._U,_U+1.._T] maps variables to linear space
	uint *_k;	// _k[1.._M] image to camera look up table where 1<=_k[m]<=K

	ftype xscale, yscale;	// applied to image coordiantes to covert to units of length

private:
	// helper functions
	void steepestdescent(ftype x[], ftype eps1, ftype eps2, uint &iter);
	void cgfr(ftype x[], ftype eps1, ftype eps2, uint& iter);
	void cgpr(ftype x[], ftype eps1, ftype eps2, uint& iter);
	void frprmn(ftype *p);

	void amoeba(ftype **p, ftype y[], uint ndim, ftype ftol, uint &nfunk);
	ftype amotry(ftype **p, ftype y[], ftype psum[], uint ndim, uint ihi, ftype fac);
	ftype const TINY;
	ftype const NMAX;
};

#endif /*NTPG_H_*/
